﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using i4pro_gruppe_d.Types;

namespace i4pro_gruppe_d.RobotCommunication
{
	class Simulator
	{
		#region Members

		public class RobotArm
		{
			#region Members
			private bool _initialized = false;
			private bool _controlOn = false;
			private bool _isHomed = false;
			private bool _hasBeenHomed = false;
			private string _previousDestination = "";
			private string _currentDestination = "";
			private readonly Random _randomizer = new Random(1337);
			#endregion

			#region Get/Set

			public bool Initialized { get; set; }
			public bool ControlOn { get; set; }
			public bool IsHomed { get; set; }
			public bool HasBeenHomed { get; set; }
			public string PreviousDestination { get; set; }
			public string CurrentDestination { get; set; }
			#endregion

			#region Commands

			//Control og Init i samme funktion bitte
			public bool Init()
			{
				Console.WriteLine("SIM - RobotArm - Initializing started.");
				Thread.Sleep(1000);
				Console.WriteLine("SIM - RobotArm - Initialized.");
				Initialized = true;
				return true;
			}

			public bool Control()
			{
				Console.WriteLine("SIM - RobotArm - Setting control on.");
				Thread.Sleep(1000);
				if (Initialized)
				{
					ControlOn = true;
					Console.WriteLine("SIM - RobotArm - Control is now on.");
					return true;
				}

				Console.WriteLine("SIM - RobotArm - FAILED!");
				Console.WriteLine("SIM - RobotArm - Initialize before setting control on.");
				return false;
			}

			public bool Home()
			{
				Console.WriteLine("SIM - RobotArm - Homing.");
				Thread.Sleep(1000);
				if (Initialized && ControlOn)
				{
					if (_randomizer.Next() % 10 != 1)
					{
						Console.WriteLine("SIM - RobotArm - Homed.");
						IsHomed = true;
						HasBeenHomed = true;
						return true;
					}
					Console.WriteLine("SIM - RobotArm - FAILED!");
					Console.WriteLine("SIM - RobotArm - Got lost trying to home.");
					return false;
				}

				Console.WriteLine("SIM - RobotArm - FAILED!");
				Console.WriteLine("SIM - RobotArm - Initialize and set control on before homing.");
				return false;
			}

			public bool MoveTo(string vectorName1, short pointNumber1, string vectorname2 = "", short pointNumber2 = (short)0)
			{
				Console.WriteLine("SIM - RobotArm - Moving RobotArm to saved point {0}", vectorName1);
				Thread.Sleep(1000);
				if (HasBeenHomed)
				{
					Console.WriteLine("SIM - RobotArm - RobotArm arrived at {0}", vectorName1);
					PreviousDestination = CurrentDestination;
					CurrentDestination = vectorName1;
					return true;
				}
				Console.WriteLine("SIM - RobotArm - FAILED!");
				Console.WriteLine("SIM - RobotArm - Home first.");
				return false;
			}

			public void Status()
			{
				Console.WriteLine("SIM - RobotArm - Status:");
				Console.WriteLine("Initialized:\t{0}", Initialized);
				Console.WriteLine("Control On:\t{0}", ControlOn);
				Console.WriteLine("Is home now:\t{0}", IsHomed);
				Console.WriteLine("Has been homed\t{0}", HasBeenHomed);
				Console.WriteLine("Previous Destination:\t{0}", PreviousDestination);
				Console.WriteLine("Current Destination:\t{0}", CurrentDestination);
			}
			#endregion
		}
		public class Gripper
		{
			#region Members
			private int _width = 0;
			private bool _gripperOpen = false;
			private readonly Random _randomizer = new Random(1337);
			#endregion

			#region Get/Set
			public int Width { get; set; }
			public bool GripperOpen { get; set; }
			#endregion

			#region Commands
			public bool OpenGripper()
			{
				if (!GripperOpen)
				{
					Console.WriteLine("SIM - Gripper - Opening gripper.");
					Thread.Sleep(1000);
					Console.WriteLine("SIM - Gripper - Gripper open.");
					GripperOpen = true;
					return true;
				}

				Console.WriteLine("SIM - Gripper - Gripper already open.");
				return true;
			}
			public bool CloseGripper()
			{
				if (GripperOpen)
				{
					Console.WriteLine("SIM - Gripper - Closing gripper.");
					Thread.Sleep(1000);
					Console.WriteLine("SIM - Gripper - Gripper closed.");
					GripperOpen = false;
					return true;
				}

				Console.WriteLine("SIM - Gripper - Gripper already closed.");
				return true;
			}
			public bool GetGripperWidth(ref short newWidth)
			{
				Console.WriteLine("SIM - Gripper - Getting gripper width.");
				Width = (_randomizer.Next() % 3) * 10 + 40;
				newWidth = (short)Width;
				Console.WriteLine("SIM - Gripper - Gripper width = {0}", Width);
				return true;
			}
			#endregion
		}
		public class LightSensor
		{
			#region Members

			private bool _isActive = false;

			#endregion

			#region Get/Set

			public bool IsActive { get; set; }

			#endregion

			#region Commands
			public bool ActivateLightSensor()
			{
				Console.WriteLine("SIM - LightSensor - Activating sensor.");
				Thread.Sleep(1000);
				if (!IsActive)
				{
					Console.WriteLine("SIM - LightSensor - Sensor activated.");
					IsActive = true;
					return true;
				}

				Console.WriteLine("SIM - LightSensor - Sensor already active.");
				return true;
			}
			public bool DeactivateLightSensor()
			{
				Console.WriteLine("SIM - LightSensor - Deactivating sensor.");
				Thread.Sleep(1000);
				if (IsActive)
				{
					Console.WriteLine("SIM - LightSensor - Sensor deactivated.");
					IsActive = false;
					return true;
				}

				Console.WriteLine("SIM - LightSensor - Sensor already deactivated.");
				return true;
			}
			#endregion
		}
		public class Conveyer
		{
			#region Members

			private readonly Random _randomizer = new Random(1337);
			private bool _isRunning = false;

			#endregion

			#region Get/Set

			public bool IsRunning { get; set; }

			#endregion

			#region Commands

			public bool StartTB()
			{
				var _lightSensor = new LightSensor();
				_lightSensor.ActivateLightSensor();
				Console.WriteLine("SIM - Conveyer - Starting Conveyer");
				Thread.Sleep(1000);
				Console.WriteLine("SIM - Conveyer - Conveyer started");
				_isRunning = true;
				Thread.Sleep(_randomizer.Next(1000, 20000));
				Console.WriteLine("- - - - - KLODS OPDAGET!!! - - - - -");
				Console.WriteLine("SIM - Conveyer - Conveyer stopped");
				Thread.Sleep(1000);
				_lightSensor.DeactivateLightSensor();
				return true;
			}

			#endregion
		}
		public class Scale
		{
			private readonly Random _randomizer = new Random(1337);

			public double MeasureWeight()
			{
				Console.WriteLine("SIM - Scale - Measuring weight");
				Thread.Sleep(1000);
				Console.WriteLine("SIM - Scale - Weight measured");
				return ((_randomizer.Next() % 6) * 10);
			}
		}

		private RobotArm _robotArm = new RobotArm();
		private Gripper _gripper = new Gripper();
		private Conveyer _conveyer = new Conveyer();
		private LightSensor _lightSensor = new LightSensor();
		private Scale _scale = new Scale();
		#endregion

		#region Singleton

		private static Simulator instance;

		private Simulator()
		{
			Console.WriteLine("Simulator created, prepare for spam!");
		}
		public static Simulator Instance
		{
			get { return instance ?? (instance = new Simulator()); }
		}

		#endregion

		#region RobotArm Calls
		public bool Init(ushort mode)
		{
			return _robotArm.Init();
		}
		public bool Control(char axis, bool status)
		{
			return _robotArm.Control();
		}
		public bool Home()
		{
			return _robotArm.Home();
		}
		public bool MoveTo(string vectorName1, short pointNumber1, string vectorname2, short pointNumber2)
		{
			return _robotArm.MoveTo(vectorName1, pointNumber1, vectorname2, pointNumber2);
		}
		public bool GetCurrentPos(ref Vector pos)
		{
			_robotArm.Status();
			return true;
		}
		#endregion

		#region Gripper Calls
		public bool OpenGripper()
		{
			if (_robotArm.HasBeenHomed)
				return _gripper.OpenGripper();

			Console.WriteLine("SIM - FAILED!");
			Console.WriteLine("SIM - Home RobotArm first.");
			return false;
		}
		public bool CloseGripper()
		{
			if (_robotArm.HasBeenHomed)
				return _gripper.CloseGripper();

			Console.WriteLine("SIM - FAILED!");
			Console.WriteLine("SIM - Home RobotArm first.");
			return false;
		}
		public bool GetGripperWidth(ref short width)
		{
			if (_robotArm.HasBeenHomed)
				return _gripper.GetGripperWidth(ref width);

			Console.WriteLine("SIM - FAILED!");
			Console.WriteLine("SIM - Home RobotArm first.");
			return false;
		}
		#endregion

		#region LightSensor Calls
		public bool ActivateLightSensor()
		{
			return _lightSensor.ActivateLightSensor();
		}
		public bool DeactivateLightSensor()
		{
			return _lightSensor.DeactivateLightSensor();
		}
		#endregion

		#region Scale Calls
		public double MeasureWeight()
		{
			return _scale.MeasureWeight();
		}

		#endregion

		#region Conveyer Calls
		public bool StartConveyer()
		{
			return _conveyer.StartTB();
		}
		public bool StopConveyer()
		{
			// Not used since StartConveyer takes care of both.
			return true;
		}
		#endregion

		public void CloseConnection()
		{
			Console.WriteLine("SIM - Connection Closed");
		}
	}
}
